#include "Constant.h"
#include "ExternHeader.h"
#include "Model.h"
#include <time.h>

// 一个从地球出发与火星交会的算例
int main()
{
	clock_t start, stop;
	start = clock();

	int j, flag = 0;
	double Out[9] = {0.0}; // 0-剩余质量，1~8-8个需要打靶的协态初值
	double rvm0[7]={
		-2.782588516572298E+10,
		1.438414801340805E+11,
		2.323875108210742E+07,
		-2.975122533379769E+04,
		-5.622821482816653E+03,
		1.663775153062375E-01,
		1200.0
	};//2000.1.1地球的日心位置和速度m, m/s，和出发质量kg
	double rv1[6]={
		-2.471320496815410E+11,
		-9.341734092972785E+09,
		5.900091859361456E+09,
		1.768440209057384E+03,
		-2.215402299903116E+04,
		-5.073508897327113E+02
	};//2000.1.1火星的日心位置和速度m, m/s
	double ft=365.25*86400.0;//要求365.25天后交会
	ft/=TUnit;//时间归一化

	//对位置和速度及质量归一化
	V_Divid(rvm0, rvm0, LUnit, 3);
	V_Divid(&rvm0[3], &rvm0[3], VUnit, 3);
	rvm0[6]/=MUnit;//质量归一化
	V_Divid(rv1, rv1, LUnit, 3);
	V_Divid(&rv1[3], &rv1[3], VUnit, 3);

	double ee0[6]={0.0}, ee1[6]={0.0};
	rv2ee(flag, ee0, rvm0, muNU);//转化为春分点轨道根数
	rv2ee(flag, ee1, rv1, muNU);
	double Lt=L0dt2Lt(ee1[5], ft, ee1, muNU);//求火星在到达时间的春分点轨道根数中随时间变化的第六个角度量L
	ee1[5]=Lt;
	/*for (j=0;j!=6;++j)
		printf("%.6f %.6f\n", ee0[j], ee1[j]);*/
	double m0=rvm0[6];
	double epsi=1.0E-5;//取定一个较小的同伦参数直接求解近似邦邦控制的结果
	int MaxGuessNum=100;//设置最大随机猜测次数
	srand( (unsigned)time( NULL ) );//设定随机数种子，若没有此设置，每次产生一样的随机数
	
	// **********
	// double ees[6] = {1.502887410, 0.598392592, -0.155582972, -0.025956131, 0.080809944, 2.191053499};
	// double eet[6] = {1.376356720, 0.496733502, -0.228968652, -0.178582339, 0.108546730, 2.382819716};
	// flag = Fixed2PFuelOpt(Out, ees, eet, m0, ft, epsi, MaxGuessNum);
	// **********
	
	flag = Fixed2PFuelOpt(Out, ee0, ee1, m0, ft, epsi, MaxGuessNum);
	printf("求解成功%d\n",flag);
	printf("剩余质量为:%.3fkg\n", Out[0]*MUnit);
	printf("lamda0为:%.6e\n", Out[1]);
	printf("7个初始协态变量值为:\n");
	for (j=2; j<=8; j++)
		printf("%.6e,\n", Out[j]);

	stop=clock();
	printf("计算用时为:%.3fs\n", (double)(stop-start)/CLOCKS_PER_SEC);
	// std:: cin >> j;
	return 0;
}